#include "RandomForests.h"




//////////////////////////////////////////////////////////////////
// ForestClassifier Object										//
//////////////////////////////////////////////////////////////////

// This constructor creates a forest by loading it from a file.
ForestClassifier::ForestClassifier(char *directory)
{
	_forest = NULL;
	ifstream input;
	input.open(directory);
	input >> _n_trees;
	_forest = new TreeClassifier[_n_trees];
	char chbuf[200];
	for(int i=0; i<_n_trees; i++)
	{
		input >> chbuf;
		_forest[i] = TreeClassifier(chbuf);
		_forest[i]._sub_w = 64;
		_forest[i]._sub_h = 128;
	}
	input.close();
}

// This function runs a forest classifier on an input image, and
// returns a list of bounding boxes.
BoundingBox* ForestClassifier::RunForestOnImage(Image* img, float scale_factor, float p_thresh)
{
	// Set variables for list of bounding boxes.
	BoundingBox* box_list_head = NULL;
	BoundingBox* box_list_end = NULL;
	
	// Set dimensions of window for first iteration.
	int scale_w = img->GetWidth();
	int scale_h = img->GetHeight();
	float scale = 1.0f;
	IntegralHist* hist = new IntegralHist(img);
	
	while((scale_w > _forest[0]._sub_w) && (scale_h > _forest[0]._sub_h))
	{
		///////////////////////////////////////////////////
		float* temp_hist = hist->_histdata;
		///////////////////////////////////////////////////

		// Loop over whole image
		for(int y=0; y<scale_h-_forest[0]._sub_h-2; y++)
		{
			for(int x=0; x<scale_w-_forest[0]._sub_w-2; x++)
			{
				// Create variable to record the score from the forest.
				float score = 0;
				// Loop over each tree in the forest
				for(int z=0; z<_n_trees; z++)
				{
					TreeNode* temp = _forest[z]._root;
					// Loop until we reach a leaf node (which has no children)
					while(temp->_l && temp->_r)
					{
						float f_val;
						// Evaluate the current feature on the image. Start by 
						// checking the feature type.
						if(temp->_f == EDGE_RATIO)
						{
							// Compute first edge feature	
							float h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*(temp->_tl_x-1)) + temp->_o1);
							float h2 = (temp->_tl_y+y == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*temp->_br_x) + temp->_o1);
							float h3 = (temp->_tl_x+x == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*(temp->_tl_x-1)) + temp->_o1);
							float h4 = *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*temp->_br_x) + temp->_o1);
							float e1 = h1 - h2 - h3 + h4;

							// Compute second edge feature
							h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*(temp->_tl_x-1)) + temp->_o2);
							h2 = (temp->_tl_y+y == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*temp->_br_x) + temp->_o2);
							h3 = (temp->_tl_x+x == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*(temp->_tl_x-1)) + temp->_o2);
							h4 = *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*temp->_br_x) + temp->_o2);
							float e2 = h1 - h2 - h3 + h4;

							// Note that an epsilon variable has been used to
							// prevent division by zero. It is also important to 
							// note that some values of e1 and e2 will be negative 
							// when they are in fact 0 due to floating point 
							// inaccuracies. The epsilon value stops small 
							// negative values from occuring, and if it is not 
							// used, other measures must be taken to compensate
							// for this.
							f_val = (e1 + EPSILON)/(e2 + EPSILON);
						}
						else
						{
							float etotal = 0;
							float e1 = 0;
							// Compute the edge features in sequence
							for(int k=0; k<ORIENTATION_BINS; k++)
							{
								float h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*(temp->_tl_x-1)) + k);
								float h2 = (temp->_tl_y+y == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*(temp->_tl_y-1)) + (ORIENTATION_BINS*temp->_br_x) + k);
								float h3 = (temp->_tl_x+x == 0) ? 0 : *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*(temp->_tl_x-1)) + k);
								float h4 = *(temp_hist + (ORIENTATION_BINS*hist->_w*temp->_br_y) + (ORIENTATION_BINS*temp->_br_x) + k);
								etotal = etotal + h1 - h2 - h3 + h4;
								if(k == temp->_o1)
									e1 = h1 - h2 - h3 + h4;
							}
							f_val = (e1 + EPSILON)/(etotal + EPSILON);
						}

						// Branch left or right depending on feature value
						if(f_val < temp->_thresh)
							temp = temp->_l;
						else
							temp = temp->_r;
					}
					// Increment the score for this tree.
					score = score + temp->_pos;
				}

				// Check if a detection occured.
				score = score/_n_trees;
				if(score > p_thresh)
				{
					if(box_list_end)
					{
						box_list_end->next = new BoundingBox;
						box_list_end->next->_x = (int)((x+1)*scale);			// x must be incremented to account for the one pixel border removed by edge detection
						box_list_end->next->_y = (int)((y+1)*scale);			// y must be incremented to account for the one pixel border removed by edge detection
						box_list_end->next->_w = (int)(_forest[0]._sub_w*scale);
						box_list_end->next->_h = (int)(_forest[0]._sub_h*scale);
						box_list_end->next->next = NULL;
						box_list_end = box_list_end->next;
					}
					else
					{
						box_list_head = new BoundingBox;
						box_list_head->_x = (int)((x+1)*scale);				// x must be incremented to account for the one pixel border removed by edge detection
						box_list_head->_y = (int)((y+1)*scale);				// y must be incremented to account for the one pixel border removed by edge detection
						box_list_head->_w = (int)(_forest[0]._sub_w*scale);
						box_list_head->_h = (int)(_forest[0]._sub_h*scale);
						box_list_head->next = NULL;
						box_list_end = box_list_head;
					}
				}

				///////////////////////////////////////////
				temp_hist += ORIENTATION_BINS;
				///////////////////////////////////////////
			}
			//////////////////////////////////////////
			temp_hist += (_forest[0]._sub_w)*ORIENTATION_BINS;
			//////////////////////////////////////////
		}
		// Resize window for next iteration
		scale_w = (int)(scale_w*(1/scale_factor) + 0.5);
		scale_h = (int)(scale_h*(1/scale_factor) + 0.5);
		delete hist;
		Image* temp = img->GetRescaledImage(scale_w,scale_h);
		temp->ConvertToGreyscale();
		hist = new IntegralHist(temp);
		delete temp;
		scale = scale*scale_factor;
		//cout << scale << endl;
	}

	delete hist;

	// Return the list of bounding boxes
	return box_list_head;
}

// This function runs a forest classifier on an input image, and
// returns a list of bounding boxes.
BoundingBox* ForestClassifier::RunForestOnImage_slow(Image* img, float scale_factor, float p_thresh)
{
	// Set variables for list of bounding boxes.
	BoundingBox* box_list_head = NULL;
	BoundingBox* box_list_end = NULL;
	
	// Set dimensions of window for first iteration.
	int scale_w = img->GetWidth();
	int scale_h = img->GetHeight();
	float scale = 1.0f;
	IntegralHist* hist = new IntegralHist(img);
	
	while((scale_w > _forest[0]._sub_w) && (scale_h > _forest[0]._sub_h))
	{

		// Loop over whole image
		for(int y=0; y<scale_h-_forest[0]._sub_h-2; y++)
		{

			for(int x=0; x<scale_w-_forest[0]._sub_w-2; x++)
			{
				// Create variable to record the score from the forest.
				float score = 0;
				// Loop over each tree in the forest
				for(int z=0; z<_n_trees; z++)
				{
					TreeNode* temp = _forest[z]._root;
					// Loop until we reach a leaf node (which has no children)
					while(temp->_l && temp->_r)
					{
						float f_val;
						// Evaluate the current feature on the image. Start by 
						// checking the feature type.
						if(temp->_f == EDGE_RATIO)
						{
							// Compute first edge feature
							float h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+temp->_o1];// Top left
							float h2 = (temp->_tl_y+y == 0) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+temp->_o1];							// Top right
							float h3 = (temp->_tl_x+x == 0) ? 0 : hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+temp->_o1];							// Bottom left
							float h4 = hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+temp->_o1];														// Bottom right						
							float e1 = h1 - h2 - h3 + h4;
							// Compute second edge feature
							h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+temp->_o2];		// Top left
							h2 = (temp->_tl_y+y == 0) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+temp->_o2];								// Top right
							h3 = (temp->_tl_x+x == 0) ? 0 : hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+temp->_o2];								// Bottom left
							h4 = hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+temp->_o2];																// Bottom right						
							float e2 = h1 - h2 - h3 + h4;
							// Note that an epsilon variable has been used to
							// prevent division by zero. It is also important to 
							// note that some values of e1 and e2 will be negative 
							// when they are in fact 0 due to floating point 
							// inaccuracies. The epsilon value stops small 
							// negative values from occuring, and if it is not 
							// used, other measures must be taken to compensate
							// for this.
							f_val = (e1 + EPSILON)/(e2 + EPSILON);
						}
						else
						{
							float etotal = 0;
							float e1 = 0;
							// Compute the edge features in sequence
							for(int k=0; k<ORIENTATION_BINS; k++)
							{
								float h1 = ((temp->_tl_y+y == 0)||(temp->_tl_x+x == 0)) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+k];	// Top left
								float h2 = (temp->_tl_y+y == 0) ? 0 : hist->_histdata[(((temp->_tl_y-1)+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+k];						// Top right
								float h3 = (temp->_tl_x+x == 0) ? 0 : hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+(((temp->_tl_x-1)+x)*ORIENTATION_BINS)+k];						// Bottom left
								float h4 = hist->_histdata[((temp->_br_y+y)*hist->_w*ORIENTATION_BINS)+((temp->_br_x+x)*ORIENTATION_BINS)+k];												// Bottom right
								etotal = etotal + h1 - h2 - h3 + h4;
								if(k == temp->_o1)
									e1 = h1 - h2 - h3 + h4;
							}
							f_val = (e1 + EPSILON)/(etotal + EPSILON);
						}

						// Branch left or right depending on feature value
						if(f_val < temp->_thresh)
							temp = temp->_l;
						else
							temp = temp->_r;
					}
					// Increment the score for this tree.
					score = score + temp->_pos;
				}

				// Check if a detection occured.
				score = score/_n_trees;
				if(score > p_thresh)
				{
					if(box_list_end)
					{
						box_list_end->next = new BoundingBox;
						box_list_end->next->_x = (int)((x+1)*scale);			// x must be incremented to account for the one pixel border removed by edge detection
						box_list_end->next->_y = (int)((y+1)*scale);			// y must be incremented to account for the one pixel border removed by edge detection
						box_list_end->next->_w = (int)(_forest[0]._sub_w*scale);
						box_list_end->next->_h = (int)(_forest[0]._sub_h*scale);
						box_list_end->next->next = NULL;
						box_list_end = box_list_end->next;
					}
					else
					{
						box_list_head = new BoundingBox;
						box_list_head->_x = (int)((x+1)*scale);				// x must be incremented to account for the one pixel border removed by edge detection
						box_list_head->_y = (int)((y+1)*scale);				// y must be incremented to account for the one pixel border removed by edge detection
						box_list_head->_w = (int)(_forest[0]._sub_w*scale);
						box_list_head->_h = (int)(_forest[0]._sub_h*scale);
						box_list_head->next = NULL;
						box_list_end = box_list_head;
					}
				}
			}
		}
		// Resize window for next iteration
		scale_w = (int)(scale_w*(1/scale_factor) + 0.5);
		scale_h = (int)(scale_h*(1/scale_factor) + 0.5);
		delete hist;
		Image* temp = img->GetRescaledImage(scale_w,scale_h);
		temp->ConvertToGreyscale();
		hist = new IntegralHist(temp);
		delete temp;
		scale = scale*scale_factor;
		cout << scale << endl;
	}

	delete hist;

	// Return the list of bounding boxes
	return box_list_head;
}
